perm filename LS1V3P.SAI[1,BGB]1 blob
sn#001257 filedate 1972-10-22 generic text, type T, neo UTF8
00100 ENTRY LS1V3P,GAP;
00200 BEGIN "LS1V3P"
00300 REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
00400 REQUIRE "TRIGER[SYS,BGB]" SOURCE_FILE;
00500 α IMPLICIT RESULTS;
00600 REAL D,WWW;
00700 α COMPONENTS OF VIEWPOINT AND LANDMARK VECTORS;
00800 REAL X,Y,Z,XA,YA,ZA,XB,YB,ZB,XC,YC,ZC;
00900 α COSINES OF RAYS TO THE 3 LANDMARK POINTS FROM THE VIEWPOINT;
01000 REAL COSα,COSβ,COSg;
01100 α COMPONENTS OF LANDMARK TRIANGLE SIDE VECTORS;
01200 REAL DXA,DYA,DZA, DXB,DYB,DZB, DXC,DYC,DZC;
01300 α THE LENGTH OF THE SIDES OF THE LANDMARK TRIANGLE;
01400 REAL A,B,C;
01500 α IMPLICIT GAP VALUES;
01600 REAL XX,ZZ;
01700 α INIT IMPLICIT GAP ARGUMENTS;
01800 REAL CTHETA,STHETA,COSPHI,SINPHI,TANPSI;
01900 α COMPUTE THE ANGLES PHI AND PSI;
02000 PROCEDURE PHIPSI (REAL Cα,Cβ,Cg);
02100 BEGIN "PHIPSI"
02200 REAL RAD,Sα,COSPSI,SINPSI,THETA,PHI,PSI;
02300 RAD ← SQRT(Cg↑2 - 2*Cα*Cβ*Cg + Cβ↑2);
02400 Sα ← SQRT(1-Cα↑2);
02500 COSPHI ← Sα*Cβ/RAD;
02600 SINPHI ← SQRT(1-COSPHI↑2);
02700 COSPSI ← RAD/Sα;
02800 SINPSI ← SQRT(1-COSPSI↑2);
02900 TANPSI ← SINPSI/COSPSI;
02950 PHI ← ACOS(COSPHI);
03000 THETA ← ACOS(Cα) - ACOS(COSPHI);
03025 PSI ← ACOS(COSPSI);
03200 STHETA ← SIN(THETA);
03300 CTHETA ← COS(THETA);
03400 END "PHIPSI";
00100 α THE GAP FUNCTION SUBROUTINE - THE CRUX OF LS1V3P IS TO FIND GAP ROOTS;
00150 REAL WHI,WLO;
00200 REAL R,S,H;
00300 INTERNAL REAL PROCEDURE GAP (REAL W);
00400 BEGIN "GAP"
00500 REAL RSW,RCW,DX,DY,K;
00600 REAL XX1,DY1,XX2,DY2;
00700 α L VECTOR;
00800 RSW ← R*SIN(W);
00900 RCW ← R*COS(W);
01000 α (B-L) VECTOR WHEN W<0, (C-L) VECTOR WHEN W≥0;
01100 DX ← -S-RCW;
01200 α PHI ROTATION CCW OF (B-L) VECTOR;
01300 BEGIN
01400 DY ← +A/2-RSW;
01500 XX1 ← COSPHI*DX - SINPHI*DY;
01600 DY1 ← COSPHI*DY + SINPHI*DX;
01700 END;
01800 α THETA ROTATION CW OF (C-L) VECTOR;
01900 BEGIN
02000 DY ← -A/2-RSW;
02100 XX2 ← CTHETA*DX + STHETA*DY;
02200 DY2 ← CTHETA*DY - STHETA*DX;
02300 END;
02500 IF W<0 THEN BEGIN DY←DY1;XX←XX1;END
02600 ELSE BEGIN DY←DY2;XX←XX2;END;
02700 α PSI ROTATION FOR Z COMPONENT;
02800 Z ← SQRT(XX↑2+DY↑2)*TANPSI;
02900 α SOLVE FOR XX & ZZ;
03000 K ← (D-RSW)/DY;
03100 XX ← RCW + K*XX;
03200 ZZ ← K*Z;
03300 RETURN(SQRT((XX+S)↑2 + ZZ↑2)- H);
03400 END "GAP";
00100 α GET THE ANSWER BACK INTO THE ORIGINAL FRAME OF REFERANCE;
00200 PROCEDURE OFRAME;
00300 BEGIN "OFRAME"
00400 DEFINE determinant (a11,a12,a13,a21,a22,a23,a31,a32,a33)=
00500 "(a11*(a22*a33-a23*a32)-a12*(a21*a33-a23*a31)+a13*(a21*a32-a22*a31))";
00600 REAL RCW,RSW,SXX,A2,RCWX,RSWD,PA2D,MA2D,II,JJ,KK,KA,KB,KC,K;
00700 REAL LA,LB,LC,G;
00800 IF ZZ<0 THEN OUTSTR("ZZ NEGATIVE."&13&10);
00900 RCW ← R*COS(WWW);
01000 RSW ← R*SIN(WWW);
01100 α COMPUTE THE LENGTHS OF THE LEGS OF THE TRIPOD;
01200 LA ← SQRT((RCW-XX)↑2 + (RSW-D)↑2 + ZZ↑2);
01300 LB ← SQRT((RCW+S)↑2 + (RSW-A/2)↑2);
01400 LC ← SQRT((RCW+S)↑2 + (RSW+A/2)↑2);
01500 α COMPUTE SOME OF THE COMPONENTS OF THE LEG VECTORS,
01600 WITH RESPECT TO THE CHORD'S MIDPOINT FRAME OF REFERENCE;
01700 A2 ← A/2;
01800 SXX ← -S-XX;
01900 RCWX ← RCW-XX;
02000 RSWD ← RSW-D;
02100 PA2D ← A2-D;
02200 MA2D ← -A2-D;
02300 α COMPONENTS OF A VECTOR NORMAL TO THE PLANE OF THE LANDMARK TRIANGLE;
02400 II ← DZB*DYC - DYB*DZC;
02500 JJ ← DXB*DZC - DZB*DXC;
02600 KK ← DYB*DXC - DXB*DYC;
02700 α COMPUTE YE OLDE TRIPLE PRODUCT OF (C TO A) CROSS (C TO B) DOT (C TO L);
02800 KA ← -DXC*XA - DYC*YA - DZC*ZA + SXX*RCWX + PA2D*RSWD +ZZ↑2;
02900 KB ← DXB*XA + DYB*YA + DZB*ZA + SXX*RCWX + MA2D*RSWD +ZZ↑2;
03000 KC ← II*XA + JJ*YA + KK*ZA + A*ZZ*(RCW + S);
03100 α APPLY CRAMER'S RULE;
03200 K ← DETERMINANT(-DXC, -DYC, -DZC,
03300 DXB, DYB, DZB,
03400 II, JJ, KK);
03500 X ← DETERMINANT( KA, -DYC, -DZC,
03600 KB, DYB, DZB,
03700 KC, JJ, KK) / K;
03800 Y ← DETERMINANT(-DXC, KA, -DZC,
03900 DXB, KB, DZB,
04000 II, KC, KK) / K;
04100 Z ← DETERMINANT(-DXC, -DYC, KA,
04200 DXB, DYB, KB,
04300 II, JJ, KC) / K;
04350 IF ABS(K)≤0.01 THEN OUTSTR("KRAMER K WARNING "&CVG(K)&13&10);
04400 END "OFRAME";
00100 α LOCUS SOLUTION: 1 VIEW OF 3 POINTS CASE;
00200 α LS1V3P RETURNS -1 FOR NO SOLUTIONS GAP LOW, 0 FOR NO SOLUTIONS GAP HIGH,
00300 OTHERWISE N FOR THAT MANY SOLUTIONS STASHED IN ARRAY V[1:N,1:3];
00400 INTERNAL INTEGER PROCEDURE LS1V3P (REAL ARRAY V,ARGS);
00500 BEGIN "LS1V3P"
00600 INTEGER NROOTS;
00700 α STASH ARGUMENT ARRAYS INTO WORKING VARIABLES;
00800 ARRBLT(XA,ARGS[1,1],12);
01200 α COMPUTE THE SIDES OF THE LANDMARK TRIANGLE;
01300 DXA ← XB - XC; DYA ← YB - YC; DZA ← ZB - ZC;
01400 DXB ← XC - XA; DYB ← YC - YA; DZB ← ZC - ZA;
01500 DXC ← XA - XB; DYC ← YA - YB; DZC ← ZA - ZB;
01600 α COMPUTE LENGTHS OF THE SIDES OF THE LANDMARK TRIANGLE;
01700 A ← SQRT(DXA↑2 + DYA↑2 + DZA↑2);
01800 B ← SQRT(DXB↑2 + DYB↑2 + DZB↑2);
01900 C ← SQRT(DXC↑2 + DYC↑2 + DZC↑2);
01902 α CHECK FOR COLINEAR CASE;
01904 BEGIN
01906 REAL A,B,C,D;
01908 A ← ACOS(COSα);
01910 B ← ACOS(COSβ);
01912 C ← ACOS(COSG);
01914 IF B>A THEN A↔B;
01916 IF C>A THEN A↔C;
01917 D ← ABS(B+C-A);
01918 IF D≤0.005 THEN OUTSTR("COLINEAR WARNING "&CVG(D)&13&10);
01920 END;
02000 BEGIN "2ND BLK"
02100 REAL SINα,COSC,ALPHA,W,GLO,GHI,G;
02200 α R IS THE RADIUS OF L'S CIRCLE;
02300 α S IS THE DISTANCE TO THE AB CHORD FROM THE CENTER OF L'S CIRCLE;
02400 SINα ← SQRT(1-COSα↑2);
02500 R ← A/(2*SINα);
02600 S ← R*COSα;
02700 α H IS THE ALTITUDE OF THE LANDMARK TRIANGLE FROM C TO SIDE AB;
02800 α H IS ALSO THE RADIUS OF THE SPINDLE;
02900 COSC ← (A↑2 + B↑2 - C↑2)/(2*A*B);
03000 H ← B*SQRT(1 - COSC↑2);
03100 α D IS THE DIRECTED DISTANCE FROM THE FOOT OF THE ALTITUDE,
03200 TO THE MIDPOINT OF THE CHORD AB;
03300 D ← B*COSC - A/2;
00100 α FIND ZERO CROSSINGS;
00200 BEGIN "3RD BLK"
00300 REAL PROCEDURE ROOT (REAL WLO,WHI);
00400 BEGIN "ROOT"
00500 REAL GLO,GHI,W,G;
00600 GLO ← GAP(WLO);
00700 GHI ← GAP(WHI);
00800 DO
00900 BEGIN "HALVE INTERVAL"
01000 W ← (WLO + WHI)/2;
01100 IF ¬(WLO<W ∧ W<WHI) THEN DONE;
01200 G ← GAP(W);
01300 IF G⊗GLO ≥ 0 THEN
01400 BEGIN
01500 GLO ← G;
01600 WLO ← W;
01700 END ELSE
01800 BEGIN
01900 GHI ← G;
02000 WHI ← W;
02100 END;
02200 END "HALVE INTERVAL" UNTIL G=0 ∨ (WHI-WLO)<1/2↑25;
02400 RETURN(W);
02500 END "ROOT";
00100 α SETUP FOR ROOT FINDING ITERATION;
00200 REAL DELTA,GOLD,G;
00300 ALPHA ← ACOS(COSα);
00400 WLO ← ALPHA - π;
00500 WHI ← π - ALPHA;
00600 DELTA ← (WHI-WLO)/200;
00700 PHIPSI(COSα,COSg,COSβ);
00800 G ← GAP(WLO);
00900 NROOTS ← 0;
01000 FOR W←WLO STEP DELTA UNTIL WHI DO
01100 BEGIN "INTERVAL"
01200 GOLD ← G;
01300 G ← GAP(W);
01400 IF G*GOLD < 0 ∧ ABS(G-GOLD)<1 THEN
01500 BEGIN "ZERO CROSSING"
01600 WWW ← ROOT(W-DELTA,W);
01700 NROOTS←NROOTS+1;
01800 OFRAME;
01900 V[NROOTS,1]← X;
02000 V[NROOTS,2]← Y;
02100 V[NROOTS,3]← Z;
02200 END "ZERO CROSSING";
02300 END "INTERVAL";
02400 RETURN(IF NROOTS≠0 THEN NROOTS ELSE IF GOLD<0 THEN -1 ELSE 0);
02500 END "3RD BLK";
02600 END "2ND BLK";
02700 END "LS1V3P";
02800
02900 END "LS1V3P";